// Meena Abdou
// CPEG 467 - ERA MOBI Robot project
// 04/04/2011

/**
 * Test the functionality of the hokuyo_aist driver from Gearbox library
 *
 * This program tests the following:
 *   Configuration: steps of the front, left, right sensors
 *   Maximum range of laser compared with max range reported by robot
 *   Laser functions   
 **/

#include <libplayerc++/playerc++.h>
#include <hokuyo_aist/hokuyo_aist.h>
#include <hokuyo_aist/hokuyo_errors.h>
#include <flexiport/flexiport.h>

#include <iostream>

#include "args.h"

#define FRONT 327 
#define RIGHT 437
#define LEFT 218

int main(int argc, char **argv)
{
  parse_args(argc,argv);

  // we throw exceptions on creation if we fail
  try
  {    
    PlayerCc::PlayerClient robot(gHostname, gPort);
    hokuyo_aist::Sensor laser;
    hokuyo_aist::SensorInfo info;
    hokuyo_aist::ScanData data;

    // Connect to laser
    laser.open("type=serial,device=/dev/ttyACM0,timeout=2");

    // Turn laser on
    laser.set_power(true);

    // Test configuring the laser
    

    // Get sensor info
    //laser.get_sensor_info(info);
    //std::cout << info.as_string() << std::endl;
    
    /** 
     *  Info from laser says:
     *
     *    Protocol: SCIP 2.0
     *    Model: URG-04LX(Hokuyo Automatic Co., Ltd.)
     *    Minimum range: 20 mm
     *    Maximum range: 5600 mm
     *    Steps in 360 degrees: 1024
     *    Scannable steps: 682
     *      First step: 44
     *      Front step: 384
     *      Last  step: 725
     *      Max   step: 768
     *    Resolution 0.00613592 radians/step
     *    Min angle: -2.08621 radians
     *    Max angle: 2.09235 radians
     *    Rotation direction: Counter-clockwise
     *
     **/

    /**
     *  Real values
     *  Since the first step is 44, all values must subtract 44
     *  to get the real value.
     *
     **/

    // Get range data
    //laser.get_ranges(data, -1, -1, 1);
    //laser.get_new_ranges(data, -1, -1, 1);
   
    //robot.Read(); //here so GetRange doesn't segfault

    double first, front, right, left;

    /**/
    // go into read-think-act loop
    for(;;)
    {
      // this blocks until new data comes; 10Hz by default
      // this has nothing to do with laser; will block entire program.
      //robot.Read();

      laser.get_ranges(data); // this fetches new data, just faster
      //laser.get_new_ranges(data, -1, -1, 1); //this one slower

      // Test the first sensor
      //first = data[44];
      //std::cout << "First: " << first << "\n";

      // Test the front sensor
      front = data[340];
      std::cout << "Front: " << front << "\n";

      // Test the right sensor
      //right = data[437];
      //std::cout << "Right: " << right << "\n";

      // Test the left sensor
      //left = data[218];
      //std::cout << "Left: " << left << "\n";
    }
    

    // Close the laser
    laser.close();
  }

  catch (hokuyo_aist::BaseError& e)
  {
    std::cerr << "Caught exception: " << e.what() << '\n';
    return 1;
  }

  catch (PlayerCc::PlayerError e)
  {
    std::cerr << e << std::endl;
    return -1;
  }
}
